//
// Created by Administrator on 12/5/2022.
//

#include "bsp.h"
#include <string.h>
#include "stdio.h"
#include "cio.h"
#include "drv_SSD1306.h"
#include "cli.h"
extern ADC_HandleTypeDef hadc1;
extern TIM_HandleTypeDef htim3;
extern TIM_HandleTypeDef htim17;
uint32_t ADC_VAL[ADC_VAL_LEN];
uint8_t RX_BUFF[RX_BUFF_MAX];
extern UART_HandleTypeDef huart1;
extern I2C_HandleTypeDef hi2c2;
const char* EXT_PKG_HEAD = "TJCFGMCP";
static int BspStepSpeed(const char *str, size_t len);
static int BspStepDst(const char *str, size_t len);
static int PrintStepper(const char *str, size_t len);
device g_step1;
device g_step2;
STEPPER_MODE g_devMode = STEPPER_POSLOCK;
#define SPEED_MAX 1000000
#define SPEED_MIN 20
#define SPEED_DEFAULT 1000

CmdPair BSP_CMD_LIST[] = {
        {"dump", Dump},
        {"speed", BspStepSpeed},
        {"to", BspStepDst},
};

void BspInit(void)
{
    CliInit(BSP_CMD_LIST, sizeof(BSP_CMD_LIST) / sizeof(CmdPair));
    CliRegKeyAction((CmdPair){CMD_KEY_F2, PrintStepper});
    g_devMode = STEPPER_POSLOCK;
    g_step1.stepCntCurr = 0;
    g_step1.stepCntDst = 0;
    g_step2.stepCntCurr = 0;
    g_step2.stepCntDst = 0;
    memset(ADC_VAL, 0, sizeof(ADC_VAL));
    BspConfigSpeed(SPEED_DEFAULT, SPEED_DEFAULT);
}

void configChannel(uint32_t ch)
{
    if (ch != ADC_CH_CURR && ch != ADC_CH_VBUS && ch != ADC_CH_EXT) {
        ch = ADC_CH_CURR;
    }

    ADC_ChannelConfTypeDef sConfig = {0};
    sConfig.Channel = ch;
    sConfig.Rank = ADC_REGULAR_RANK_1;
    sConfig.SamplingTime = ADC_SAMPLINGTIME_COMMON_1;

    HAL_ADC_Stop_DMA(&hadc1);
    if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK)
    {
        Error_Handler();
    }
    HAL_ADC_Start_DMA(&hadc1, ADC_VAL, ADC_VAL_LEN);
}

uint8_t validPkg(const pkg* s)
{
    if (s == NULL) {
        return HAL_ERROR;
    }
    if (memcmp(s->head, PKG_HEAD, strlen(PKG_HEAD)) == 0) {
        return HAL_OK;
    } else {
        return HAL_ERROR;
    }

}

uint32_t bk_falsh_GetPage(uint32_t addr)
{
    uint32_t page = 0;

    if (addr < (FLASH_BASE + FLASH_BANK_SIZE)) {
        /* Bank 1 */
        page = (addr - FLASH_BASE) / FLASH_PAGE_SIZE;
    } else {
        /* Bank 2 */
        page = (addr - (FLASH_BASE + FLASH_BANK_SIZE)) / FLASH_PAGE_SIZE;
    }

    return page;
}

#define ROM_ADDR 0x0800fd00

void writeROM(const uint8_t* write_data, uint32_t data_size)
{
    uint16_t i,m,n;
    uint64_t data;
    uint32_t index;

    if(data_size == 0)return;
    if(data_size * 4 > FLASH_PAGE_SIZE)data_size = FLASH_PAGE_SIZE / 4;			// 数据大小不能大于1页的大小

// 1 解锁flash
    HAL_FLASH_Unlock();

// 2 擦除FLASH
// 初始化FLASH_EraseInitTypeDef
    FLASH_EraseInitTypeDef f;
    f.TypeErase = FLASH_TYPEERASE_PAGES;					// 擦除方式页擦除
    f.Page = bk_falsh_GetPage(ROM_ADDR);					// 擦除的起始页
    f.Banks = FLASH_BANK_1;					// 擦除的flash区域
    f.NbPages = 1;											// 擦除1页
// 设置PageError
    uint32_t PageError = 0;
// 调用擦除函数
    HAL_FLASHEx_Erase(&f, &PageError);

// 3 取反写FLASH
    n = data_size % 8;
    if(n == 0)												// 数据长度能被8整除
        m = data_size / 8 ;									// 得到写入flash的双字的个数
    else
        m = data_size / 8 + 1;

    for(i=0;i<m;i++)
    {
        data = 0;
        if(i != (m-1) || (i == (m-1) && n == 0))
        {
            *((uint8_t *)&data + 0) = write_data[0 + i*8];
            *((uint8_t *)&data + 1) = write_data[1 + i*8];
            *((uint8_t *)&data + 2) = write_data[2 + i*8];
            *((uint8_t *)&data + 3) = write_data[3 + i*8];
            *((uint8_t *)&data + 4) = write_data[4 + i*8];
            *((uint8_t *)&data + 5) = write_data[5 + i*8];
            *((uint8_t *)&data + 6) = write_data[6 + i*8];
            *((uint8_t *)&data + 7) = write_data[7 + i*8];
        }
        else
        {
            *((uint8_t *)&data + 0) = write_data[0 + i*8];
            if(n >= 2)
                *((uint8_t *)&data + 1) = write_data[1 + i*8];
            if(n >= 3)
                *((uint8_t *)&data + 2) = write_data[2 + i*8];
            if(n >= 4)
                *((uint8_t *)&data + 3) = write_data[3 + i*8];
            if(n >= 5)
                *((uint8_t *)&data + 4) = write_data[4 + i*8];
            if(n >= 6)
                *((uint8_t *)&data + 5) = write_data[5 + i*8];
            if(n >= 7)
                *((uint8_t *)&data + 6) = write_data[6 + i*8];
        }

        index = ROM_ADDR + i*8;

        HAL_FLASH_Program(FLASH_TYPEPROGRAM_DOUBLEWORD, index, data);		// 如果不想取反写入Flash 把取反符号删除即可
    }


// 4 锁定FLASH
    HAL_FLASH_Lock();
}

void readROM(uint8_t* read_data, uint32_t data_size)
{

    uint16_t i;
    uint32_t index;

    for(i=0;i<data_size;i++)
    {
        index = ROM_ADDR + i;
        if(index < (FLASH_BASE + FLASH_BANK_SIZE))
            read_data[i] = *((__IO uint8_t *)index);
        else
            read_data[i] = 0;
    }
}

uint32_t CioTxBytes(const uint8_t* tx, uint32_t len)
{
    return HAL_UART_Transmit(&huart1, (uint8_t*)tx, len, 99);
}

uint32_t IIC_WRByte(uint8_t devAddr, uint8_t regAddr, uint8_t dat)
{
    uint8_t data[2]  = {regAddr, dat};
    return HAL_I2C_Master_Transmit(&hi2c2, devAddr, data, 2, 99);
}

uint32_t IIC_RDByte(uint8_t devAddr, uint8_t regAddr, uint8_t* data, uint8_t dataLen)
{
    HAL_StatusTypeDef ret = HAL_I2C_Master_Transmit(&hi2c2, devAddr, &regAddr, 1, 5);
    ret |= HAL_I2C_Master_Receive(&hi2c2, devAddr, data, dataLen, 99);
    if (ret == HAL_OK) {
        return ERR_SUCCESS;
    } else {
        return ERR_ACKMISS;
    }
}

void OLED_Delay(uint32_t ms)
{
    HAL_Delay(ms);
}

void CliPutChar(char ch)
{
    while (!(USART1->ISR & UART_FLAG_TXE)) {}
    USART1->TDR = ch;
}

int Dump(const char *str, size_t len)
{
    UNUSED(str);
    UNUSED(len);

    LOG("DEVID %08x-%08x-%08x", HAL_GetUIDw0(), HAL_GetUIDw1(), HAL_GetUIDw2());
    PrintStepper(NULL, 0);
    return ERR_SUCCESS;
}

int BspWriteIICBus(const char *str, size_t len)
{
    if (str == NULL || len == 0) {
        return ERR_BADPARA;
    }
    uint32_t devAddr = 0, regAddr = 0, data = 0;
    sscanf(str, "%u %u %u", &devAddr, &regAddr, &data);
    LOG("WRITE dev[%u]|reg[%u], data[%u]...", devAddr, regAddr, data);
    uint32_t ret = IIC_WRByte(devAddr, regAddr, data);
    if (ret == HAL_OK) {
        LOG("ok");
    } else {
        LOG("ack missed");
    }

    return ERR_SUCCESS;
}

int BspReadIICBus(const char *str, size_t len)
{
    if (str == NULL || len == 0) {
        return ERR_BADPARA;
    }
    uint32_t devAddr = 0, regAddr = 0, datalen = 0;
    sscanf(str, "%u %u %u", &devAddr, &regAddr, &datalen);
    LOG("READ dev[%u]|reg[%u], datalen[%u]...", devAddr, regAddr, datalen);
    uint8_t data[128];
    uint32_t ret = IIC_RDByte(devAddr, regAddr, data, datalen);
    if (ret == HAL_OK) {
        for (size_t idx = 0; idx < datalen; idx++) {
            LOG("reg %u = %u", regAddr + idx, data[idx]);
        }
    } else {
        LOG("ack missed");
    }

    return ERR_SUCCESS;
}
static int PrintStepper(const char *str, size_t len)
{
    (void)str; (void)len;
    LOGTop("pos[%6d,%6d]->[%6d,%6d], speed[%5d,%5d], stick[%4d,%4d] | %s",
           g_step1.stepCntCurr, g_step2.stepCntCurr,
           g_step1.stepCntDst, g_step2.stepCntDst,
           g_step1.stepSpeed, g_step2.stepSpeed,
           g_step1.stickerInput, g_step2.stickerInput,
           g_devMode == STEPPER_POSLOCK ? "POSLOCK" : "SPEEDLOCK");
    FlushRx();
    return ERR_SUCCESS;
}

static int BspStepSpeed(const char *str, size_t len)
{
    if (str == NULL || len == 0) {
        return ERR_BADPARA;
    }
    uint32_t speed = 0;
    sscanf(str, "%u",&speed);
    BspConfigSpeed(speed, speed);
    LOG("Config Speed to %d", speed);
    PrintStepper(NULL, 0);

    return ERR_SUCCESS;
}

void EnableStepper(void)
{
    if (g_step1.stepCntCurr != g_step1.stepCntDst) {
        HAL_TIM_PWM_Start(&htim17, TIM_CHANNEL_1);
    }
    if (g_step2.stepCntCurr != g_step2.stepCntDst) {
        HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_4);
    }
}

static int BspStepDst(const char *str, size_t len)
{
    if (str == NULL || len == 0) {
        return ERR_BADPARA;
    }
    sscanf(str, "%d %d", &g_step1.stepCntDst, &g_step2.stepCntDst);
    if (g_devMode != STEPPER_POSLOCK) {
        g_devMode = STEPPER_POSLOCK;
        BspConfigSpeed(SPEED_DEFAULT, SPEED_DEFAULT);
    }
    LOG("Config Dst to [%d,%d]", g_step1.stepCntDst, g_step2.stepCntDst);
    PrintStepper(NULL, 0);
    EnableStepper();

    return ERR_SUCCESS;
}

const char* CliTimeStamp()
{
    static char time[] = "00:00:00.000";
    uint32_t tick = HAL_GetTick();
		uint32_t ms = tick % 1000;
    uint32_t second = (tick / 1000) % 60;
    uint32_t min = (tick / 1000 / 60) % 60;
    uint32_t hour = (tick / 1000 / 60 / 60) % 24;
    sprintf(time, "%2u:%02u:%02u.%03u", hour, min, second, ms);
    return time;
}

int ModifyCurve(uint32_t raw)
{
    const int deadZone = 150;
    const int fullScale = 1 << 10;
    int dst = (int)raw - fullScale / 2;
    if (dst > deadZone) {
        dst -= deadZone;
    } else if (dst < -1 * deadZone) {
        dst += deadZone;
    } else {
        dst = 0;
    }

    return dst / 20;
}

void BspUpdateAdcInput()
{
    const size_t channelMax = 4;
    uint32_t sumX = 0, sumY = 0;
    for (size_t idx = 0; idx < ADC_VAL_LEN / channelMax; idx++) {
        sumX += ADC_VAL[idx * channelMax + 1];
        sumY += ADC_VAL[idx * channelMax + 3];
    }
    uint32_t inputX = sumX / (ADC_VAL_LEN / channelMax);
    uint32_t inputY = sumY / (ADC_VAL_LEN / channelMax);
    g_step1.stickerInput = ModifyCurve(inputX);
    g_step2.stickerInput = ModifyCurve(inputY);
    if (g_step1.stickerInput == 0 && g_step2.stickerInput == 0) {
        return;
    }
    g_devMode = STEPPER_SPEEDLOCK;
    g_step1.stepCntDst += g_step1.stickerInput;
    g_step2.stepCntDst += g_step2.stickerInput;
    EnableStepper();
}

void BspConfigSpeed(uint32_t speedX, uint32_t speedY)
{
    CLAMP_VALUE(speedX, SPEED_MIN, SPEED_MAX - 1);
    CLAMP_VALUE(speedY, SPEED_MIN, SPEED_MAX - 1);
    g_step1.stepSpeed = speedX;
    TIM3->ARR = SPEED_MAX / g_step1.stepSpeed - 1;
    TIM3->CCR4 = TIM3->ARR / 2;
    g_step2.stepSpeed = speedY;
    TIM17->ARR = SPEED_MAX / g_step2.stepSpeed - 1;
    TIM17->CCR1 = TIM17->ARR / 2;
}
